#ifndef __MGGS_LSM__
#define __MGGS_LSM__
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>
#include <eigen/Eigen/Dense>

using namespace Eigen;

template<int n_coe>
class function{
protected:
  double input;
  double output;
public:
  function();
  virtual ~function() = default;
  void load_input(const double _input);
  void load_output(const double _output);
  double get_input() const;
  double get_output() const;
  virtual double cal_func(double _input, double* _coes) = 0;
  virtual void cal_func(double* _coes) = 0;
  virtual double* cal_dfunc(double _input, double* _coes) =0;
};


class GLB_function: public function<2>{
private:
  double* c_coes;
public:
  GLB_function();
  ~GLB_function();
  virtual double cal_func(double _input, double* _coes);
  virtual void cal_func(double* _coes);
  virtual double* cal_dfunc(double _input, double* _coes);
  double get_c_coes(int _index) const;
  double van_laar1(const double _input, const double* _coes);
  double van_laar2(const double _input, const double* _coes);
  double Antoine1();
  double Antoine2();
};

template<int n_coe, int n_data>
class LSM{
private:
  double** data;
  GLB_function* func;
public:
  LSM();
  LSM(const std::string &_file);
  ~LSM();
  int load_data(const std::string &_file);
  double** get_data() const;
  int list_data() const;
  GLB_function* get_func() const;
  Vector<double,n_data> r(double* _coes);
  double f(Vector<double,n_data> _r);
  Matrix<double,n_data,n_coe> J(double* _coes);
  Vector<double,n_coe> df(double* _coes);
  double rho(double* _coes, Vector<double,n_coe> _p, double _lamda);
  double* LM(double* _coes, double tol , double lamda);
};





template<int n_coe>
function<n_coe>::function(){};


template<int n_coe>
void function<n_coe>::load_input(const double _input){
  input = _input;
}

template<int n_coe>
void function<n_coe>::load_output(const double _output){
  output = _output;
}

template<int n_coe>
double function<n_coe>::get_input() const{
  return input;
}

template<int n_coe>
double function<n_coe>::get_output() const{
  return output;
}


GLB_function::GLB_function(){
  c_coes = new double[6];
  std::fstream os("Antoine_coefficients");
  for (int i = 0; i < 6 ; i++)
    os >> c_coes[i];
  os.close();
}


GLB_function::~GLB_function(){
  delete[] c_coes;
}


double GLB_function::get_c_coes(int _index) const{
  return c_coes[_index];
}

double GLB_function::van_laar1(const double _input, const double* _coes){
  double tmp = _coes[1]*(1-_input)/(_coes[0]*_input+_coes[1]*(1-_input));
  return exp(_coes[0]*tmp*tmp);
}

double GLB_function::van_laar2(const double _input, const double* _coes){
  double tmp = _coes[0]*_input/(_coes[0]*_input+_coes[1]*(1-_input));
  return exp(_coes[1]*tmp*tmp);
}

double GLB_function::Antoine1(){
  return pow(10,c_coes[0] - c_coes[1]/(20+c_coes[2]));
}

double GLB_function::Antoine2(){
  return pow(10,c_coes[3] - c_coes[4]/(20+c_coes[5]));
}



double GLB_function::cal_func(double _input, double* _coes){
  double v1 = van_laar1(_input,_coes);
  double v2 = van_laar2(_input,_coes);
  double p1 = Antoine1();
  double p2 = Antoine2();
  return _input*v1*p1+(1-_input)*v2*p2;
}

void GLB_function::cal_func(double* _coes){
  this->load_output(cal_func(this->get_input(),_coes));
}


double* GLB_function::cal_dfunc(double _input, double* _coes){
  double* result = new double[2];
  double v1 = van_laar1(_input,_coes);
  double v2 = van_laar2(_input,_coes);
  double p1 = Antoine1();
  double p2 = Antoine2();
  double tmp0 = _input * _coes[0] + (1-_input) * _coes[1];
  double tmp1 = (1-_input)*(1-_input)*_coes[1]*_coes[1];
  double tmp2 = _input*_input*_coes[0]*_coes[0];
  result[0] = _input*p1*v1*(tmp1/(tmp0*tmp0)-2*_coes[0]*_input*tmp1/(tmp0*tmp0*tmp0))
    +(1-_input)*p2*v2*2*_input*_input*_coes[0]*_coes[1]*_coes[1]*(1-_input)/(tmp0*tmp0*tmp0);
  result[1] = _input*p1*v1*2*(1-_input)*(1-_input)*_coes[1]*_coes[0]*_coes[0]*_input/(tmp0*tmp0*tmp0)
    +(1-_input)*p2*v2*(tmp2/(tmp0*tmp0)-2*(1-_input)*_coes[1]*tmp2/(tmp0*tmp0*tmp0));
  return result;
}


template<int n_coe, int n_data>
LSM<n_coe,n_data>::LSM(){
  data = NULL;
  func = new GLB_function;
}

template<int n_coe, int n_data>
LSM<n_coe,n_data>::LSM(const std::string &_file){
  this->load_data(_file);
  func = new GLB_function;
}

template<int n_coe, int n_data>
LSM<n_coe,n_data>::~LSM(){
  if (data != NULL){
    for(int i = 0; i < n_data ; i++)
      delete []data[i];
    delete []data;
  }
  if (func != NULL)
    delete func;
}


template<int n_coe, int n_data>
int LSM<n_coe,n_data>::load_data(const std::string &_file){
  std::fstream _data(_file);
  int N;
  _data >> N;
  if (N != n_data){
    std::cerr << "The number of datas no matching!" << std::endl;
    exit(-1);
  }
  data =(double**) new double*[n_data];
  for (int i = 0 ; i < N ; i++)
    data[i] = new double[2];
  for (int i = 0 ; i < N ; i++)
    _data >> data[i][0] >> data[i][1];
  _data.close();
  return 0;
}

template<int n_coe, int n_data>
double** LSM<n_coe,n_data>::get_data() const{
  return data;
}

template<int n_coe, int n_data>
int LSM<n_coe,n_data>::list_data() const{
  if (data == NULL){
    std::cerr << "No datas!" << std::endl;
    exit(-1);
  }
  for (int i = 0 ; i < n_data ; i++)
    std::cout << data[i][0] << " , " << data[i][1] << std::endl;
  return 0;
}

template<int n_coe, int n_data>
GLB_function* LSM<n_coe,n_data>::get_func() const{
  return func;
}


template<int n_coe, int n_data>
Vector<double,n_data> LSM<n_coe,n_data>::r(double* _coes){
  Vector<double,n_data> res;
  for (int i = 0 ; i < n_data ; i++)
    res(i) = func->cal_func(data[i][0],_coes)-data[i][1];
  return res;
}

template<int n_coe, int n_data>
double LSM<n_coe,n_data>::f(Vector<double,n_data> _r){
  double sum = 0;
  for (int i = 0 ; i < n_data ; i++)
    sum += _r(i)*_r(i);
  return sum*0.5;
}

template<int n_coe, int n_data>
Matrix<double,n_data,n_coe> LSM<n_coe,n_data>::J(double* _coes){
  Matrix<double,n_data,n_coe> res;
  for (int i = 0 ; i < n_data ; i++){
    double* dfunc = func->cal_dfunc(data[i][0],_coes);
    res(i,0) = dfunc[0];
    res(i,1) = dfunc[1];
  }
  return res;
}

template<int n_coe, int n_data>
Vector<double,n_coe> LSM<n_coe,n_data>::df(double* _coes){
  Matrix<double,n_data,n_coe> J = this->J(_coes);
  Vector<double,n_data> r = this->r(_coes);
  Vector<double,n_coe> res = J.transpose()*r;
  return res;
}

template<int n_coe, int n_data>
double  LSM<n_coe,n_data>::rho(double* _coes, Vector<double,n_coe> _p, double _lamda){
  double* coes_n = new double[n_coe];
  for (int i = 0 ; i < n_coe ; i++)
    coes_n[i] = _coes[i] + _p(i);
  double f = this->f(this->r(_coes));
  double f_n = this->f(this->r(coes_n));
  Vector<double,n_coe> df = this->df(coes_n);
  double dm = 0.5*_p.transpose()*(df-_lamda*_p)+f_n-f;
  return (f_n - f)/dm;
}

template<int n_coe, int n_data>
double* LSM<n_coe,n_data>::LM(double* _coes, double tol , double lamda){
  double f=this->f(this->r(_coes));
  double* coes_n = new double[n_coe];
  for (int i = 0 ; i < n_coe ; i++)
    coes_n[i] = _coes[i];
  int k = 0;
  while (f > tol && k < 1000){
    Matrix<double,n_data,n_coe> J = this->J(coes_n);
    Matrix<double,n_coe,n_coe> A = J.transpose()*J+lamda*MatrixXd::Identity(n_coe, n_coe);
    Vector<double,n_coe> df = this->df(coes_n);
    Vector<double,n_coe> p = A.colPivHouseholderQr().solve(-df);
    double rho = this->rho(coes_n,p,lamda);
    if (rho < 0.25)
      lamda = 4 * lamda;
    if (rho > 0.75)
      lamda = 0.5 * lamda;
    if (rho > 0.1){
      for (int i = 0 ; i < n_coe ; i++)
	coes_n[i]+=p(i);
      f = this->f(this->r(coes_n));
    }
    k++;
  }
  return coes_n;
}

#else
//do nothing
#endif
